/* Nano fiber Scanner Control "Firmware"
  
   TODO:
   - Maybe add the automatic mode to step through
     
     
   The circuit:
	* Polarizer Servo pins PB1(D9) PB2(D10)
	* Encoder Wheel pins PC0(A0) PC1(A1)
	* UV LEDs pin PD5(D5)
        * White LEDs pin PD6(D6)
        
   Communication Protocol:
   Command  Action
     g      report
       -p      wheel position (angle/count) and servo speed
       -l      LEDs status
     s      set
       -s    servo/motor (Set_Servo_Speedo[1000-2000] : "ss1380")
       -a    angle       (Set_Angle_Number[0-315] : "sa120")
       -l    LEDs
         -u  UV          (Set_Light_UV_Brightness[0-255] : "slu20")
         -w  White       (Set_Light_White_Brightness[0-255] : "slw200")


   created 15 May 2011
   by Sergejus Martinenas
   modified 18 June 2011
   by Sergejus Martinenas
 
   This code is in the public domain.

 */


#include <Servo.h> 

Servo p0Servo;
//Servo p1Servo;  //uncomment if both servos are used

// Constants:
const int servo0Pin = 10;  //Main servo pin - used
const int servo1Pin = 9; //Reserved servo pin

const int uvLEDpin = 5;
const int whiteLEDpin = 6;

const int encoder0PinA = A0; //Base signal
const int encoder0PinB = A1; //Direction signal

const int baudrate = 9600;

//const int WHEEL_COUNT = 720;

const int CW_MAX = 1460;
const int CW = 1410;
const int STOP = 1380;
const int CCW = 1350;
const int CCW_MAX = 1300;

const int LED_ON = 255;
const int LED_OFF = 0;

const int MAX_INT = 4000;

//Variables:
int n = LOW;
int encoder0PinALast = LOW;
int encoder0Pos = 0;

int current_angle;
int set_angle;
int diff_angle;
int current_count;

boolean setFlag;


int servo0speed;

int uv_status = 0;
int white_status = 0;

//Setup the kit
void setup() {
  //constrain the variables
  current_angle = 0;
  set_angle = 0;
  
  servo0speed = STOP;
  setFlag = false;
  
  //initialize serial communication:
  Serial.begin(baudrate);
  
  //attach the servo
  p0Servo.attach(servo0Pin);
  p0Servo.writeMicroseconds(STOP);
  
  //setup the output pins
  pinMode(uvLEDpin, OUTPUT);
  pinMode(whiteLEDpin, OUTPUT);
  
  //setup the input pins
  pinMode(encoder0PinA, INPUT);  
  pinMode(encoder0PinB, INPUT);
  
  //turn off UV and White LEDs (blink White, then UV LEDs for 1s)
  analogWrite(whiteLEDpin, LED_ON);
  delay(1000);
  analogWrite(whiteLEDpin, LED_OFF);
  analogWrite(uvLEDpin, LED_ON);
  delay(1000);
  analogWrite(uvLEDpin, LED_OFF);
  calibrate();
  Serial.println("BPRO Ready");
}

void loop()
{
  readEncoder();                           //read position   
  if (Serial.available() > 0) {            //check is serial data is received
    p0Servo.writeMicroseconds(STOP);       //STOP motor
    getSerialCommand();                    //Get Command from Serial port
    //readEncoder();                             
  }
  if (setFlag) {
    turnToSetAngle();
  }
  setValues();
}

void calibrate(void)
{
  //implement calibration routine
  p0Servo.writeMicroseconds(CW_MAX);
  delay(2000);
  zero();
}

void setValues(void)  //set LED values
{
  p0Servo.writeMicroseconds(servo0speed);
  analogWrite(uvLEDpin, uv_status);
  analogWrite(whiteLEDpin, white_status);
}

void readEncoder()  //reads and updates encoder counts and current turn angle
{
  n = digitalRead(encoder0PinA);
   if ((encoder0PinALast == LOW) && (n == HIGH)) {
     if (digitalRead(encoder0PinB) == LOW) {
       encoder0Pos--;
     } else {
       encoder0Pos++;
     }
     current_count = encoder0Pos;
     current_angle = current_count/2;
   }
   diff_angle = current_angle - set_angle;
   encoder0PinALast = n;
}

void turnToSetAngle()  //Turns the polarizer wheel to the the specified angle
{
  //read current angle and calculate the difference
  readEncoder();
  if(diff_angle != 0)
  {    
    readEncoder();
    if (diff_angle > 0)
    servo0speed = CW + 10;
    else
    servo0speed = CCW - 10;
  } else {
    //p0Servo.writeMicroseconds(STOP);
    servo0speed = STOP;
    setFlag = false;
    Serial.println("#");
  }
}

int readNumber()    //Reads number from the Serial port
{
  int number=0, digit=0;
  digit=Serial.read();
  while ((((int)digit-48)>=0)&&(((int)digit-48)<=9)&&(number<MAX_INT)) {
    number*=10;
    number+=(int)digit-48;
    digit=Serial.read();
  }
  return number;
}

void zero()
{
  p0Servo.writeMicroseconds(STOP);
  encoder0Pos = 0;
  current_count = 0;
  current_angle = 0;
  set_angle = 0;
  servo0speed = STOP;
}

void getSerialCommand()
{
  int tmp_number;
  switch(Serial.read()) {
//***************************************************************************************************
    case 'z': {  //zero the position
      zero();
      Serial.println("Zero");      
      break;
    }
    case 'c': {
      calibrate();
      Serial.println("Calibrated");
      break;
    }
    case 'm': {
      set_angle = 314;
      setFlag = true;
      Serial.println("Mirror");
      break;
    }
//***************************************************************************************************
    case 'q': {  //turn CCW
      //p0Servo.writeMicroseconds(CCW-10);
      servo0speed = CCW-10;
      Serial.println("CCW");
      break;
    }
    case 'w': {  //STOP turn and report position
      servo0speed = STOP;
      //p0Servo.writeMicroseconds(STOP);
      setFlag = false;
      Serial.print("STOP: ");
      Serial.println(current_angle);
      break;
    }
    case 'e': {  //turn CW
      //p0Servo.writeMicroseconds(CW+10);
      servo0speed = CW+10;
      Serial.println("CW");
      break;
    }
//***************************************************************************************************
    case 's': {  //Set routine
      Serial.print("Set ");
      switch(Serial.read()) {
        case 'a': {  //Set angle
          Serial.print("Angle ");
          tmp_number = readNumber();
          set_angle = constrain(tmp_number, 0, 314);//tmp_number;
          setFlag = true;
          Serial.println(set_angle);
          break;
        }
        case 'l': {  //Set light
          Serial.print("LEDs ");
          switch(Serial.read()) {
            case 'u': {  //Set UV light
              Serial.print("UV ");
              tmp_number = readNumber();
              uv_status = constrain(tmp_number, LED_OFF, LED_ON);
              Serial.println(uv_status);
              break;
            }
            case 'w': {  //Set White light
              Serial.print("W "); //
              tmp_number=readNumber();
              white_status = constrain(tmp_number, LED_OFF, LED_ON);//tmp_number;
              Serial.println(white_status);
              break;
            }
          }
          break;
        }
        case 's': {  //Set Servo Speed
          Serial.print("Servo ");
          tmp_number = readNumber();
          servo0speed = constrain(tmp_number, CCW_MAX, CW_MAX);// tmp_number;  
          p0Servo.writeMicroseconds(servo0speed);
          Serial.println(servo0speed);
          break;
        }
      }
      break;
    }
//***************************************************************************************************
    case 'g': {  //Report routine
      Serial.println("Report ");
      switch(Serial.read()) {
        case 'p': {  //Report Position, Counts and Servo speed
          Serial.println("Pos:");
          Serial.println(current_angle);
          Serial.println(set_angle);
          Serial.println(diff_angle);
          Serial.println(servo0speed);
          Serial.println(setFlag, BIN);
          break;
        }
        case 'l': {  //Report LED status
          Serial.print("LEDs ");
          Serial.print(uv_status);
          Serial.print(" ");
          Serial.println(white_status);
          break;
        }
        case 'a': {
          Serial.println(current_angle);
        }
      }
      break;
    }
    case 'h': {  //print Help
      Serial.println("Help");
      break;
    }
  }
}